// --------------------------------------------------------------------------------------------------------------------
// <copyright file="FloorFilterPrunerTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Runtime
{
    using System;
    using System.Collections.Generic;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Tests EV4FloorFilterDataPruner pruner
    /// </summary>
    [TestClass]
    public class FloorFilterPrunerTests
    {
        /// <summary>
        /// Tests if Pruner Reports Correct State When Not Initialized
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PrunerReportsCorrectStateWhenNotInitialized()
        {
            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0, 0, 0, 0, 0);

            Assert.IsFalse(pruner.IsInitialized());
        }

        /// <summary>
        /// Tests if Pruner Reports Correct State When Initialized
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PrunerReportsCorrectStateWhenInitialized()
        {
            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0, 0, 0, 0, 0);

            pruner.SetCalibrationData(FloorFilterTestHelper.GetPerfectCalibrationData());

            Assert.IsTrue(pruner.IsInitialized());
        }

        /// <summary>
        /// Tests if PrunerValidatorThrowsIfSessionWasNotInitialized
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(InvalidOperationException))]
        public void PrunerValidatorThrowsIfSessionWasNotInitialized()
        {
            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0.7, -0.1, 100, 10, 5);

            double[][] calibrationData = FloorFilterTestHelper.GetRealisticCalibrationData(60, 80);

            pruner.IsLearnDataValid();
        }

        /// <summary>
        /// Tests if Pruner Validator Reports Correct Status Depending On The Number Of Samples
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PrunerValidatorReportsCorrectStatusDependingOnTheNumberOfSamples()
        {
            int width = 80;
            int height = 60;

            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0.7, -0.1, 100, 10, 5);

            double[][] calibrationData = FloorFilterTestHelper.GetRealisticCalibrationData(height, width);

            pruner.SetCalibrationData(calibrationData);

            FastTuple<double, double, double> floorFitResult = new FastTuple<double, double, double>(height / 2, 40000, 0.1);            

            int stripCount = calibrationData[0].Length;
            int stripWidth = 32; // does not really matter what the value is. In this test we are not verifying fitting algo

            pruner.PrepareForNewPruningSession(height, width, stripCount);

            for (int i = 0; i < stripCount; ++i)
            {
                int stripCenter = i * stripWidth + stripWidth / 2;

                pruner.AddSample(stripCenter, floorFitResult, 1200);

                if (i + 1 > stripCount / 4)
                {
                    Assert.IsTrue(pruner.IsLearnDataValid());
                }
                else 
                {
                    Assert.IsFalse(pruner.IsLearnDataValid());
                }
            }
        }

        /// <summary>
        /// Tests if PruneOutliersDoesNotPruneGoodValues
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PruneOutliersDoesNotPruneWhenLearnDataIsGood()
        {
            int width = 80;
            int height = 60;

            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0.7, -0.1, 100, 10, 5);

            double[][] calibrationData = FloorFilterTestHelper.GetRealisticCalibrationData(height, width);

            pruner.SetCalibrationData(calibrationData);            

            int stripCount = calibrationData[0].Length;
            int stripWidth = 32; // does not really matter what the value is. In this test we are not verifying fittin algo

            pruner.PrepareForNewPruningSession(height, width, stripCount);

            for (int i = 0; i < stripCount; ++i)
            {
                int stripCenter = i * stripWidth + stripWidth / 2;

                FastTuple<double, double, double> floorFitResult = new FastTuple<double, double, double>(height / 2, 40000, 0.1);

                // add sample values- they will have no variance, so all should be preserved
                pruner.AddSample(stripCenter, floorFitResult, 1200);
            }

            pruner.PruneOutliers();

            List<FastTuple<double, double>> dataPointsA, dataPointsB, dataPointsD;

            pruner.GetPrunedDataPoints(out dataPointsA, out dataPointsB, out dataPointsD);

            Assert.AreEqual(stripCount, dataPointsA.Count);
            Assert.AreEqual(stripCount, dataPointsB.Count);
            Assert.AreEqual(stripCount, dataPointsD.Count);
        }

        /// <summary>
        /// Tests if PruneOutliersDoesPruningWhenSomeOfLearnDataIsBad
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void PruneOutliersDoesPruningWhenSomeOfLearnDataIsBad()
        {
            int width = 80;
            int height = 60;

            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0.7, -0.1, 100, 10, 5);

            double[][] calibrationData = FloorFilterTestHelper.GetRealisticCalibrationData(height, width);

            pruner.SetCalibrationData(calibrationData);

            int stripCount = calibrationData[0].Length;
            int stripWidth = 32; // does not really matter what the value is. In this test we are not verifying fittin algo

            pruner.PrepareForNewPruningSession(height, width, stripCount);

            int stripCenter = 0;

            // we will add good data for all but 5 last strips, for which we add bad data of different sorts after the loop
            for (int i = 0; i < stripCount - 5; ++i)
            {
                stripCenter = i * stripWidth + stripWidth / 2;

                FastTuple<double, double, double> floorFitResult = new FastTuple<double, double, double>(height / 2, 40000, 0.1);

                // add sample values- they will have no variance, so all should be preserved
                pruner.AddSample(stripCenter, floorFitResult, 0);
            }

            // Add one screwed up value for Variance
            FastTuple<double, double, double> badVariance = new FastTuple<double, double, double>(height / 2, 40000, 2);
            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, badVariance, 0);

            // Add one screwed up value for too big a D
            FastTuple<double, double, double> badD = new FastTuple<double, double, double>(height / 2, 45000, 0.1);
            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, badD, 0);

            // Add one screwed up value for too small a D
            FastTuple<double, double, double> badD2 = new FastTuple<double, double, double>(height / 2, 35000, 0.1);
            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, badD2, 0);

            // Add one screwed up value for too big an A
            FastTuple<double, double, double> badA = new FastTuple<double, double, double>(height * 0.8, 40000, 0.1);
            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, badA, 0);

            // Add one screwed up value for too small an A
            FastTuple<double, double, double> badA2 = new FastTuple<double, double, double>(height * -0.2, 40000, 0.1);
            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, badA2, 0);

            // Pruner should eliminate last 5 entries
            pruner.PruneOutliers();

            List<FastTuple<double, double>> dataPointsA, dataPointsB, dataPointsD;

            pruner.GetPrunedDataPoints(out dataPointsA, out dataPointsB, out dataPointsD);

            Assert.AreEqual(stripCount - 5, dataPointsA.Count);
            Assert.AreEqual(stripCount - 5, dataPointsB.Count);
            Assert.AreEqual(stripCount - 5, dataPointsD.Count);
        }

        /// <summary>
        /// Tests if PruneOutliersDoesPruningWhenSomeOfLearnDataIsBad
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void LearnValidatorReturnsTrueWhenDataIsStable()
        {
            int width = 80;
            int height = 60;

            Ev4FloorDataPruner pruner = new Ev4FloorDataPruner(0.7, -0.1, 100, 10, 5);

            double[][] calibrationData = FloorFilterTestHelper.GetRealisticCalibrationData(height, width);

            int stripCount = calibrationData[0].Length;
            int stripWidth = 20; // does not really matter what the value is. In this test we are not verifying fittin algo
            int stripCenter = 10; 

            pruner.SetCalibrationData(calibrationData);

            pruner.PrepareForNewPruningSession(height, width, stripCount);

            // now add screwed up values
            FastTuple<double, double, double> dummyStripData = new FastTuple<double, double, double>(0, 0, 0.1);
            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, dummyStripData, 1200);

            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, dummyStripData, 1400);

            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, dummyStripData, 1600);

            bool smoothLineValidation = pruner.IsLearnDataValid();

            Assert.IsTrue(smoothLineValidation);

            stripCenter += stripWidth;
            pruner.AddSample(stripCenter, dummyStripData, 1200); // add a second surface. It should triger an alarm

            bool jaggedLineValidation = pruner.IsLearnDataValid();

            Assert.IsFalse(jaggedLineValidation);
        }
    }
}
